package org.nashua.tt151;


import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.SimpleRobot;
import org.nashua.tt151.Controller.DualAction;

public class ShooterTest extends SimpleRobot {
    public Dashboard dash;
    public DualAction driver = new DualAction(1);
    public DualAction shooter = new DualAction(2);
    public Jaguar sTop = new Jaguar(RobotMap.Jaguar.SHOOTER_TOP);
    public Jaguar sBot = new Jaguar(RobotMap.Jaguar.SHOOTER_BOTTOM);
    public Servo yaw = new Servo(9);
    public DigitalOutput led = new DigitalOutput(1);
    public void robotInit() {
        try {
            dash = new Dashboard("10.1.53.5",null);
            System.out.println(dash.requestInfo("ECHO Test123"));
        } catch (Exception e){}
    }
    public void autonomous(){
        yaw.set(0.5);
        while (true){
            double offset = Integer.parseInt(dash.requestInfo("OFFSET-T0"));
            System.out.println("Offset = "+offset+"px");
            offset *= 0.01;
            System.out.println("Yaw += "+offset+"");
            yaw.set(Math.max(0.4,Math.min(0.6,yaw.getPosition()+offset)));
            System.out.println("Yaw = "+yaw.get());
        }
    }
    public void operatorControl() {
        double current=0;
        double target=0;
        yaw.set(0.5);
        while (isOperatorControl()&&isEnabled()) {
            led.set(driver.getOption(DualAction.Button.A));
            if (driver.wasReleased(DualAction.Button.LEFT_BUMPER)) target-=0.1;
            if(driver.wasReleased(DualAction.Button.RIGHT_BUMPER)) target+=0.1;
            if (driver.wasReleased(DualAction.Button.LEFT_TRIGGER)) target--;
            if(driver.wasReleased(DualAction.Button.RIGHT_TRIGGER)) target++;
            if (driver.wasReleased(DualAction.Button.BACK)) target=0;
            if (driver.wasReleased(DualAction.Button.B)) yaw.setPosition(Math.max(0.4,yaw.getPosition()-0.1));
            if (driver.wasReleased(DualAction.Button.Y)) yaw.setPosition(Math.min(0.6,yaw.getPosition()+0.1));
            if (driver.wasReleased(DualAction.Button.X)) yaw.setPosition(Math.max(0.4,yaw.getPosition()-0.01));
            if (driver.wasReleased(DualAction.Button.A)) yaw.setPosition(Math.min(0.6,yaw.getPosition()+0.01));
            System.out.println("Servo: "+yaw.getPosition());
            if (target<0) target=0;
            if (target>12) target=12;
            current+=(target-current)/100;
            sTop.set(-current/12);
            sBot.set(-current/12);
            System.out.println(current+"V ("+(current/12)+")");
            try {
                dash.updatePWM("Output",1,"Top",current+"V ("+(current/12)+")");
                dash.updatePWM("Output",2,"Bottom",current+"V ("+(current/12)+")");
            } catch (Exception e){}
        }
    }
}
